import multiprocessing


class DeltaPID(object):
    """双环PID控制深度"""

    def __init__(self, model):
        self.control_dic = None
        self.model = model
        self.Kp_1 = 0.8
        self.Ki_1 = 0
        self.Kd_1 = 1
        self.Kp_2 = 5
        self.Ki_2 = 0
        self.Kd_2 = 0.3

        self.pid_out = 0

        self.error_depth = 0
        self.error_theta = 0
        self.error_depth_sum = 0
        self.error_theta_sum = 0

        self.count = 0

    def calculate(self, target, cur_val, depth_dot, theta, theta_dot):

        self.error_depth = target - cur_val
        self.depth_dot = depth_dot
        self.error_depth_sum = self.error_depth_sum + self.error_depth
        expect_theta = self.Kp_1 * self.error_depth + self.Ki_1 * self.error_depth_sum - self.Kd_1 * self.depth_dot

        self.error_theta = theta - expect_theta
        self.theta_dot = theta_dot
        self.error_theta_sum = self.error_theta_sum + self.error_theta
        self.pid_out = self.Kp_2 * self.error_theta + self.Ki_2 * self.error_theta_sum - self.Kd_2 * self.theta_dot

        return self.pid_out

    def list_sum(self, list_input):
        list_sum = 0
        for i in range(len(list_input)):
            list_sum += list_input[i]

        return list_sum



    def Saturation(self, value, limit_up, limit_down):
        if value > limit_up:
            return limit_up
        elif value < limit_down:
            return limit_down
        else:
            return value

